// getERTFromF.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "Matrix3d.h"
#include "Matrix.h"
#include "CalObservation2dC.h"
#include "Gemoetry.h"
#include <vector>
#include <iostream>
#include <fstream>
#include <io.h>
#include <string>
#include <cv.h>
using namespace std;
#pragma comment(lib,"opencv_core220d.lib")
#pragma comment(lib,"opencv_imgproc220d.lib")
#pragma comment(lib,"opencv_highgui220d.lib")
#pragma comment(lib,"opencv_calib3d220d.lib")
#pragma comment(lib,"opencv_legacy220d.lib")
int inputpoint(string datap,vector<CalObservation2dArrayC>& obs)
{

	int ncameras=0;
	char fnPoints[256];
	datap=datap+"points";
	do
	{
		sprintf(fnPoints, "%s.%02d.txt",datap.c_str() , ncameras++);
	}
	while (access(fnPoints, 04) == 0);
	--ncameras;

	if (ncameras == 0)
	{
		cerr << "ERROR: " << fnPoints << " not found" << endl;
		return 0;
	}
	cerr << "Loading data for " << ncameras << " cameras." << endl;
	obs.resize(ncameras);
	for (int icam = 0; icam < ncameras; icam++)
	{
		sprintf(fnPoints, "%s.%02d.txt", datap.c_str(), icam);
		ifstream filePoints(fnPoints);
		if (!filePoints.good())
		{
			cerr << "ERROR: couldn't open " << fnPoints << endl;
			return 0;
		}

		int rows = 0;
		filePoints >> rows;    
		obs[icam] = CalObservation2dArrayC(rows);
		for (int r = 0; r < rows; r++)
		{
			float u, v;
			filePoints >> u >> v;
			obs[icam][r] = CalObservation2dC(u,v,((u > 0) && (v > 0)) ? true : false);
		}
		// all views must have same number
		if(obs[icam].Size()!= obs[0].Size())
			return 0;
	}

	return 1;
}
CvMat* GetFundamentalMat(vector<CalObservation2dArrayC>& obs,int icam,int jcam,CvMat** fundamental_mat)
{
	int point_count=obs[icam].Size();
	CvMat* point1=cvCreateMat(1,point_count,CV_32FC2);
	CvMat* point2=cvCreateMat(1,point_count,CV_32FC2);
	CvMat* status=cvCreateMat(1,point_count,CV_8UC1);
	CvMat* fundamental_matrix=cvCreateMat(3,3,CV_32FC1);
	*fundamental_mat=fundamental_matrix;
	cvSetZero(point1);cvSetZero(point2);
	int ivisible=0;
	for (int i=0;i<point_count;i++)
	{    
		if (obs[icam][i].visible&&obs[jcam][i].visible)
		{
			point1->data.fl[ivisible*2]  =obs[icam][i].u[0];
			point1->data.fl[ivisible*2+1]=obs[icam][i].u[1];
			point2->data.fl[ivisible*2]  =obs[jcam][i].u[0];
			point2->data.fl[ivisible*2+1]=obs[jcam][i].u[1];
			ivisible++;
		}
	}
	cvSave("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\point0.xml",point1);
	cvSave("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\point1.xml",point2);
	if(point1->cols>8)
		cvFindFundamentalMat(point1,point2,fundamental_matrix,CV_FM_RANSAC,1.0,0.99,status);
	if(point1->cols==8)
		cvFindFundamentalMat(point1,point2,fundamental_matrix,CV_FM_8POINT,1.0,0.99,status);
	cvSave("dd.xml",fundamental_matrix);
	cout<<cvSum(status).val[0]<<endl;
	return status;
}
int _tmain(int argc, _TCHAR* argv[])
{
    inputpoint("G:\\luan\\test\\600\\data2\\",obs); 
	Matrix3dC E;
	Matrix3dC R;
	Matrix3dC K0;
	Matrix3dC K1;
	Matrix3dC F;
	Vector2dC C0(320,240);
	Vector2dC C1(320,240);
	Vector3dC T;
	float fxy1=1;
	float fxy2=1;
	CvMat* fundaptr=NULL;
	CvMat* status=GetFundamentalMat(obs,0,1,&fundaptr);
	FundamentalMatrixToEssential(E,K0,K1,F,C0,C1,1,1);
	EssentialMatrixDecompose(R,T,E,K0,K1,obs[0][0],obs[1][0]);
	cvReleaseMat(&fundaptr);
	cvReleaseMat(&status);
	return 0;
}

